feat(commander): uncommanded altitude loss detection with parachute f… by gguidone · Pull Request #26837 · PX4/PX4-Autopilot

gguidone · 2026-03-20T09:53:10Z

Solved Problem

Rotary-wing vehicles with a parachute system have no dedicated failsafe
for uncommanded altitude loss (e.g. motor failure, structural failure).
The existing fd_critical_failure path covers attitude limits and
external ATS, but not altitude drop while the vehicle is still
attitude-stable. This PR adds that missing detection path.

Solution

FailureDetector::updateAltitudeStatus is called each cycle when
attitude control is active. It compares vehicle_local_position.z
against vehicle_local_position_setpoint.z using a ratcheting
reference that tracks the highest altitude reached while below the
setpoint. When the drop exceeds FD_ALT_LOSS for FD_ALT_LOSS_T
seconds, failure_detector_status.flags.alt is set, which propagates
to FailsafeFlags.fd_alt_loss and triggers Action::Terminate in the
failsafe framework. Commander then sets actuator_armed.termination = true and calls send_parachute_command() on the rising edge.

Changelog Entry

New feature: uncommanded altitude loss detection for rotary-wing vehicles.
When FD_ALT_LOSS > 0, a drop exceeding the threshold while altitude
control is active immediately triggers flight termination and parachute
deployment via COM_ALT_LOSS_ACT.
New parameters: FD_ALT_LOSS, FD_ALT_LOSS_T, COM_ALT_LOSS_ACT

Test coverage

Build verified: make px4_sitl_default, make px4_fmu-v6x_default
Unit tests pass: make tests TESTFILTER=failsafe_test
Bench-tested in SITL: detection triggers correctly on simulated altitude drop, Terminate fires and parachute command is sent

github-actions · 2026-03-23T09:32:25Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 752 byte (0.04 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +752  +0.0%    +752    .text
  [NEW]    +268  [NEW]    +268    FailureDetector::updateAltitudeStatus()
   +31%    +120   +31%    +120    FailureDetector::FailureDetector()
  +8.1%    +108  +8.1%    +108    Failsafe::checkStateAndMode()
  +0.1%     +96  +0.1%     +96    g_cromfs_image
   +68%     +52   +68%     +52    FailureDetector::update()
  +0.0%     +32  +0.0%     +32    [section .text]
   +23%     +24   +23%     +24    FailureDetector::updateParamsImpl()
   +21%     +20   +21%     +20    FailureDetector::~FailureDetector()
  +2.5%     +20  +2.5%     +20    ucdr_serialize_failsafe_flags()
  +0.1%     +16  +0.1%     +16    px4::parameters
  +3.4%     +12  +3.4%     +12    Commander::handleAutoDisarm()
  +0.5%      +8  +0.5%      +8    Commander::Commander()
  +2.3%      +8  +2.3%      +8    Commander::~Commander()
  +1.7%      +8  +1.7%      +8    FailureDetectorChecks::checkAndReport()
  [NEW]      +4  [NEW]      +4    CSWTCH.828
  +0.7%      +4  +0.7%      +4    Commander::arm()
 -100.0%      +4 -100.0%      +4    [15 Others]
  [DEL]      -4  [DEL]      -4    CSWTCH.824
  -0.2%      -8  -0.2%      -8    Commander::handle_command()
  -6.7%     -12  -6.7%     -12    Commander::updateControlMode()
  -5.0%     -28  -5.0%     -28    Commander::control_status_leds()
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%      +8  [ = ]       0    .debug_aranges
+0.0%     +68  [ = ]       0    .debug_frame
+0.0% +8.99Ki  [ = ]       0    .debug_info
+0.0% +1.11Ki  [ = ]       0    .debug_line
  [DEL]      -6  [ = ]       0    [Unmapped]
  +0.0% +1.12Ki  [ = ]       0    [section .debug_line]
+0.0% +1.06Ki  [ = ]       0    .debug_loclists
+0.0%    +101  [ = ]       0    .debug_rnglists
+0.1% +2.02Ki  [ = ]       0    .debug_str
+0.8%      +2  [ = ]       0    .shstrtab
+0.0%     +90  [ = ]       0    .strtab
  [DEL]     -11  [ = ]       0    CSWTCH.824
  [NEW]     +11  [ = ]       0    CSWTCH.828
  [NEW]     +90  [ = ]       0    FailureDetector::updateAltitudeStatus()
+0.0%     +64  [ = ]       0    .symtab
  [DEL]     -32  [ = ]       0    CSWTCH.824
  [NEW]     +32  [ = ]       0    CSWTCH.828
   +17%     +16  [ = ]       0    Commander::Commander()
 -50.0%     -16  [ = ]       0    Commander::checkForMissionUpdate()
  +100%     +16  [ = ]       0    Commander::checkWorkerThread()
 -25.0%     -16  [ = ]       0    Commander::control_status_leds()
  +100%     +16  [ = ]       0    Commander::updateControlMode()
 -33.3%     -16  [ = ]       0    Commander::updateTunes()
 -50.0%     -32  [ = ]       0    EKFGSF_yaw::fuseVelocity()
  +300%     +48  [ = ]       0    FailureDetector::update()
  [NEW]     +48  [ = ]       0    FailureDetector::updateAltitudeStatus()
 -25.0%     -32  [ = ]       0    RcvTopicsPubs::init()
  +0.3%     +32  [ = ]       0    [section .symtab]
  -1.5%     -16  [ = ]       0    uORB::Publication<>::publish()
   +14%     +16  [ = ]       0    uORB::SubscriptionData<>::SubscriptionData()
-7.5%    -752  [ = ]       0    [Unmapped]
+0.0% +13.6Ki  +0.0%    +752    TOTAL

px4_fmu-v6x [Total VM Diff: 744 byte (0.04 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +744  +0.0%    +744    .text
  [NEW]    +268  [NEW]    +268    FailureDetector::updateAltitudeStatus()
   +31%    +120   +31%    +120    FailureDetector::FailureDetector()
  +8.1%    +108  +8.1%    +108    Failsafe::checkStateAndMode()
  +0.1%     +96  +0.1%     +96    g_cromfs_image
   +68%     +52   +68%     +52    FailureDetector::update()
  +0.0%     +32  +0.0%     +32    [section .text]
   +23%     +24   +23%     +24    FailureDetector::updateParamsImpl()
   +21%     +20   +21%     +20    FailureDetector::~FailureDetector()
  +2.5%     +20  +2.5%     +20    ucdr_serialize_failsafe_flags()
  +0.1%     +16  +0.1%     +16    px4::parameters
  +3.4%     +12  +3.4%     +12    Commander::handleAutoDisarm()
  +0.5%      +8  +0.5%      +8    Commander::Commander()
  +2.3%      +8  +2.3%      +8    Commander::~Commander()
  +1.7%      +8  +1.7%      +8    FailureDetectorChecks::checkAndReport()
  [NEW]      +4  [NEW]      +4    CSWTCH.828
  +0.7%      +4  +0.7%      +4    Commander::arm()
  [DEL]      -4  [DEL]      -4    CSWTCH.824
 -100.0%      -4 -100.0%      -4    [15 Others]
  -0.2%      -8  -0.2%      -8    Commander::handle_command()
  -6.7%     -12  -6.7%     -12    Commander::updateControlMode()
  -5.0%     -28  -5.0%     -28    Commander::control_status_leds()
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%      +8  [ = ]       0    .debug_aranges
+0.0%     +68  [ = ]       0    .debug_frame
+0.0% +8.86Ki  [ = ]       0    .debug_info
+0.0% +1.12Ki  [ = ]       0    .debug_line
   +75%      +3  [ = ]       0    [Unmapped]
  +0.0% +1.11Ki  [ = ]       0    [section .debug_line]
+0.0% +1.00Ki  [ = ]       0    .debug_loclists
+0.0%    +104  [ = ]       0    .debug_rnglists
  [NEW]      +3  [ = ]       0    [Unmapped]
  +0.0%    +101  [ = ]       0    [section .debug_rnglists]
+0.1% +2.04Ki  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%     +90  [ = ]       0    .strtab
  [DEL]     -11  [ = ]       0    CSWTCH.824
  [NEW]     +11  [ = ]       0    CSWTCH.828
  [NEW]     +90  [ = ]       0    FailureDetector::updateAltitudeStatus()
+0.0%     +64  [ = ]       0    .symtab
  [DEL]     -32  [ = ]       0    CSWTCH.824
  [NEW]     +32  [ = ]       0    CSWTCH.828
   +17%     +16  [ = ]       0    Commander::Commander()
 -50.0%     -16  [ = ]       0    Commander::checkForMissionUpdate()
  +100%     +16  [ = ]       0    Commander::checkWorkerThread()
 -25.0%     -16  [ = ]       0    Commander::control_status_leds()
  +100%     +16  [ = ]       0    Commander::updateControlMode()
 -33.3%     -16  [ = ]       0    Commander::updateTunes()
  +100%     +32  [ = ]       0    EKFGSF_yaw::fuseVelocity()
  +300%     +48  [ = ]       0    FailureDetector::update()
  [NEW]     +48  [ = ]       0    FailureDetector::updateAltitudeStatus()
 -25.0%     -32  [ = ]       0    RcvTopicsPubs::init()
  -0.3%     -32  [ = ]       0    [section .symtab]
  -1.5%     -16  [ = ]       0    uORB::Publication<>::publish()
   +14%     +16  [ = ]       0    uORB::SubscriptionData<>::SubscriptionData()
 +74% +3.27Ki  [ = ]       0    [Unmapped]
+0.0% +17.4Ki  +0.0%    +744    TOTAL

Updated: 2026-03-30T09:38:55

src/modules/commander/commander_params.yaml

bresch

Did you check if this also doesn't trigger when the EKF does an altitude reset?

msg/FailsafeFlags.msg

src/modules/commander/failure_detector/FailureDetector.cpp

bresch · 2026-04-10T07:40:58Z

Could you maybe take the opportunity to add a unit test for this?

…ailsafe Detects when a rotary-wing vehicle drops more than FD_ALT_LOSS metres below a NED-z reference while altitude control is active, and immediately triggers flight termination (parachute deployment). Detection (FailureDetector): - FD_ALT_LOSS: drop threshold in metres (0 = disabled, default) - FD_ALT_LOSS_T: hysteresis time - Guards: rotary-wing only, altitude control active, z_valid, setpoint fresh (<1 s). Manual, Acro and FW/VTOL-FW modes are excluded. - Ratcheting reference: initialises to lpos.z on first sample below setpoint, preventing false triggers on new waypoints Failsafe action (commander): - New fd_alt_loss flag in FailsafeFlags.msg - COM_ALT_LOSS_ACT: -1=Disabled (default), 0=Terminate - Terminate fires immediately, cannot be overridden, and never clears until disarm (parachute deployment is irreversible)

src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp

github-actions · 2026-04-13T09:09:13Z

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 840 byte (0.04 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +840  +0.0%    +840    .text
    [NEW]    +348  [NEW]    +348    FailureDetector::updateAltitudeStatus()
     +32%    +124   +32%    +124    FailureDetector::FailureDetector()
    +8.1%    +108  +8.1%    +108    Failsafe::checkStateAndMode()
    +0.1%     +96  +0.1%     +96    g_cromfs_image
     +68%     +52   +68%     +52    FailureDetector::update()
    +0.0%     +36  +0.0%     +36    [section .text]
     +23%     +24   +23%     +24    FailureDetector::updateParamsImpl()
     +21%     +20   +21%     +20    FailureDetector::~FailureDetector()
    +2.5%     +20  +2.5%     +20    ucdr_serialize_failsafe_flags()
    +0.1%     +16  +0.1%     +16    px4::parameters
    +3.4%     +12  +3.4%     +12    Commander::handleAutoDisarm()
    +0.5%      +8  +0.5%      +8    Commander::Commander()
    +2.3%      +8  +2.3%      +8    Commander::~Commander()
    +1.7%      +8  +1.7%      +8    FailureDetectorChecks::checkAndReport()
    [NEW]      +4  [NEW]      +4    CSWTCH.828
    +0.7%      +4  +0.7%      +4    Commander::arm()
    [DEL]      -4  [DEL]      -4    CSWTCH.824
    -1.6%      -4  -1.6%      -4    Commander::checkForMissionUpdate()
    -6.7%     -12  -6.7%     -12    Commander::updateControlMode()
    -5.0%     -28  -5.0%     -28    Commander::control_status_leds()
  +0.0%    +102  [ = ]       0    .debug_abbrev
  +0.0%      +8  [ = ]       0    .debug_aranges
  +0.0%     +68  [ = ]       0    .debug_frame
  +0.0% +9.00Ki  [ = ]       0    .debug_info
  +0.0% +1.22Ki  [ = ]       0    .debug_line
     +50%      +1  [ = ]       0    [Unmapped]
    +0.0% +1.22Ki  [ = ]       0    [section .debug_line]
  +0.0% +1.15Ki  [ = ]       0    .debug_loclists
  +0.0%     +98  [ = ]       0    .debug_rnglists
    [DEL]      -3  [ = ]       0    [Unmapped]
    +0.0%    +101  [ = ]       0    [section .debug_rnglists]
  +0.1% +2.02Ki  [ = ]       0    .debug_str
  -0.8%      -2  [ = ]       0    .shstrtab
  +0.0%     +90  [ = ]       0    .strtab
    [DEL]     -11  [ = ]       0    CSWTCH.824
    [NEW]     +11  [ = ]       0    CSWTCH.828
    [NEW]     +90  [ = ]       0    FailureDetector::updateAltitudeStatus()
  +0.0%     +64  [ = ]       0    .symtab
    [DEL]     -32  [ = ]       0    CSWTCH.824
    [NEW]     +32  [ = ]       0    CSWTCH.828
     +17%     +16  [ = ]       0    Commander::Commander()
   -50.0%     -16  [ = ]       0    Commander::checkForMissionUpdate()
    +100%     +16  [ = ]       0    Commander::checkWorkerThread()
   -25.0%     -16  [ = ]       0    Commander::control_status_leds()
    +100%     +16  [ = ]       0    Commander::updateControlMode()
   -33.3%     -16  [ = ]       0    Commander::updateTunes()
    +300%     +48  [ = ]       0    FailureDetector::update()
    [NEW]     +48  [ = ]       0    FailureDetector::updateAltitudeStatus()
   -25.0%     -32  [ = ]       0    RcvTopicsPubs::init()
     +25%     +16  [ = ]       0    matrix::Matrix<>::operator+=()
   -25.0%     -16  [ = ]       0    matrix::Matrix<>::operator/()
    -1.5%     -16  [ = ]       0    uORB::Publication<>::publish()
     +14%     +16  [ = ]       0    uORB::SubscriptionData<>::SubscriptionData()
  -8.7%    -840  [ = ]       0    [Unmapped]
  +0.0% +13.8Ki  +0.0%    +840    TOTAL

px4_fmu-v6x [Total VM Diff: 928 byte (0.05 %)]

    FILE SIZE        VM SIZE    
 --------------  -------------- 
  +0.0%    +928  +0.0%    +928    .text
    [NEW]    +348  [NEW]    +348    FailureDetector::updateAltitudeStatus()
     +32%    +124   +32%    +124    FailureDetector::FailureDetector()
    +8.1%    +108  +8.1%    +108    Failsafe::checkStateAndMode()
    +0.1%     +96  +0.1%     +96    g_cromfs_image
   -99.3%     +88 -99.3%     +88    [34 Others]
     +68%     +52   +68%     +52    FailureDetector::update()
    +0.0%     +36  +0.0%     +36    [section .text]
     +23%     +24   +23%     +24    FailureDetector::updateParamsImpl()
     +21%     +20   +21%     +20    FailureDetector::~FailureDetector()
    +2.5%     +20  +2.5%     +20    ucdr_serialize_failsafe_flags()
    +0.1%     +16  +0.1%     +16    px4::parameters
    +3.4%     +12  +3.4%     +12    Commander::handleAutoDisarm()
    +0.5%      +8  +0.5%      +8    Commander::Commander()
    +2.3%      +8  +2.3%      +8    Commander::~Commander()
    +1.7%      +8  +1.7%      +8    FailureDetectorChecks::checkAndReport()
    [NEW]      +4  [NEW]      +4    CSWTCH.828
    +0.7%      +4  +0.7%      +4    Commander::arm()
    [DEL]      -4  [DEL]      -4    CSWTCH.824
    -1.6%      -4  -1.6%      -4    Commander::checkForMissionUpdate()
    -6.7%     -12  -6.7%     -12    Commander::updateControlMode()
    -5.0%     -28  -5.0%     -28    Commander::control_status_leds()
  +0.0%    +102  [ = ]       0    .debug_abbrev
  +0.0%      +8  [ = ]       0    .debug_aranges
  +0.0%     +68  [ = ]       0    .debug_frame
  +0.0% +8.88Ki  [ = ]       0    .debug_info
  +0.0% +1.25Ki  [ = ]       0    .debug_line
   -33.3%      -2  [ = ]       0    [Unmapped]
    +0.0% +1.25Ki  [ = ]       0    [section .debug_line]
  +0.0% +1.13Ki  [ = ]       0    .debug_loclists
  +0.0%    +108  [ = ]       0    .debug_rnglists
    [NEW]      +3  [ = ]       0    [Unmapped]
    +0.0%    +105  [ = ]       0    [section .debug_rnglists]
  +0.1% +2.04Ki  [ = ]       0    .debug_str
  +0.9%      +2  [ = ]       0    .shstrtab
  +0.0%     +90  [ = ]       0    .strtab
    [DEL]     -11  [ = ]       0    CSWTCH.824
    [NEW]     +11  [ = ]       0    CSWTCH.828
    [NEW]     +90  [ = ]       0    FailureDetector::updateAltitudeStatus()
  +0.0%     +64  [ = ]       0    .symtab
    [DEL]     -32  [ = ]       0    CSWTCH.824
    [NEW]     +32  [ = ]       0    CSWTCH.828
     +17%     +16  [ = ]       0    Commander::Commander()
   -50.0%     -16  [ = ]       0    Commander::checkForMissionUpdate()
    +100%     +16  [ = ]       0    Commander::checkWorkerThread()
   -25.0%     -16  [ = ]       0    Commander::control_status_leds()
    +100%     +16  [ = ]       0    Commander::updateControlMode()
   -33.3%     -16  [ = ]       0    Commander::updateTunes()
   -50.0%     -16  [ = ]       0    ConstLayer::contains()
     +50%     +16  [ = ]       0    ConstLayer::store()
   -20.0%     -16  [ = ]       0    DynamicSparseLayer::DynamicSparseLayer()
    +300%     +48  [ = ]       0    FailureDetector::update()
    [NEW]     +48  [ = ]       0    FailureDetector::updateAltitudeStatus()
   -25.0%     -32  [ = ]       0    RcvTopicsPubs::init()
    +0.7%     +80  [ = ]       0    [section .symtab]
     +25%     +16  [ = ]       0    matrix::Matrix<>::operator+=()
   -25.0%     -16  [ = ]       0    matrix::Matrix<>::operator/()
   -50.0%     -16  [ = ]       0    param_for_index
   -50.0%     -16  [ = ]       0    param_get_index
   -50.0%     -32  [ = ]       0    param_hash_check
 -15.6%    -928  [ = ]       0    [Unmapped]
  +0.0% +13.7Ki  +0.0%    +928    TOTAL

Updated: 2026-04-13T11:35:32

bresch

Looks good, thanks for the unit tests!

gguidone force-pushed the gg/fd-altitude-loss-detection branch from e340af4 to 91a4720 Compare March 23, 2026 09:24

MaEtUgR reviewed Mar 27, 2026

View reviewed changes

src/modules/commander/commander_params.yaml Outdated Show resolved Hide resolved

gguidone force-pushed the gg/fd-altitude-loss-detection branch 4 times, most recently from 8fea79c to 68e47df Compare March 30, 2026 09:31

bresch requested changes Apr 10, 2026

View reviewed changes

gguidone requested a review from bresch April 13, 2026 07:41

gguidone force-pushed the gg/fd-altitude-loss-detection branch from 68e47df to 8542f09 Compare April 13, 2026 09:01

github-actions bot reviewed Apr 13, 2026

View reviewed changes

gguidone force-pushed the gg/fd-altitude-loss-detection branch from 8542f09 to 9461b37 Compare April 13, 2026 09:35

test(failure_detector): add unit tests for altitude loss detection

9ac4ff0

gguidone force-pushed the gg/fd-altitude-loss-detection branch from 9461b37 to 9ac4ff0 Compare April 13, 2026 11:30

bresch approved these changes Apr 13, 2026

View reviewed changes

gguidone merged commit 4c1078c into main Apr 13, 2026
78 checks passed

gguidone deleted the gg/fd-altitude-loss-detection branch April 13, 2026 12:23

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(commander): uncommanded altitude loss detection with parachute f…#26837

feat(commander): uncommanded altitude loss detection with parachute f…#26837
gguidone merged 2 commits intomainfrom
gg/fd-altitude-loss-detection

gguidone commented Mar 20, 2026

Uh oh!

github-actions bot commented Mar 23, 2026 •

edited

Loading

Uh oh!

Uh oh!

bresch left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bresch commented Apr 10, 2026

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions bot commented Apr 13, 2026 •

edited

Loading

Uh oh!

bresch left a comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

gguidone commented Mar 20, 2026

Solved Problem

Solution

Changelog Entry

Test coverage

Uh oh!

github-actions bot commented Mar 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🔎 FLASH Analysis

Uh oh!

Uh oh!

bresch left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bresch commented Apr 10, 2026

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

github-actions bot commented Apr 13, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

🔎 FLASH Analysis

Uh oh!

bresch left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

github-actions bot commented Mar 23, 2026 •

edited

Loading

github-actions bot commented Apr 13, 2026 •

edited

Loading